classdef Normal_PP_Controller < handle
    properties (Access = public)
        Lf = 0.0;
        Lr = 0.0;
        El = 0.0;
        Ld = 0.0;
        ExpAng = 0.0;
        Sign = 0;
    end
    methods (Access = public)
        function obj = Normal_PP_Controller(VehLf,VehLr)
            obj.Lf = VehLf;
            obj.Lr = VehLr;
        end
        
        function Cal_Lateral_Dist(obj,VehP,TrackP,yaw)
            x_d = TrackP.x - VehP.x;
            y_d = TrackP.y - VehP.y;
            el = -sin(yaw) * x_d + cos(yaw) * y_d;
            obj.Sign = -el/abs(el);
            obj.El = abs(el);
        end
        
        function Cal_Euclid_Dist(obj,VehP,TrackP)
            obj.Ld = ((TrackP.x - VehP.x)^2 + (TrackP.y - VehP.y)^2)^0.5;
        end
        
        function AngleRate = Control_Process(obj,VehARR,TrackP,Delta_t)
            obj.Cal_Lateral_Dist(VehARR.P_m,TrackP,VehARR.P_f.yaw);
            obj.Cal_Euclid_Dist(VehARR.P_m,TrackP);
            exp_steer_wheel_angle = ((2*(obj.Lf + obj.Lr)*obj.El)/((obj.Ld)^2));%atan((2*(obj.Lf + obj.Lr)*obj.El)/((obj.Ld)^2));
            angle_deviation = (obj.Sign * exp_steer_wheel_angle) - VehARR.steer_wheel_angle;
            obj.ExpAng = exp_steer_wheel_angle;
            AngleRate = angle_deviation/Delta_t;
            fprintf('Veh pf (%f ,%f) pr (%f ,%f)\n',VehARR.P_f.x,VehARR.P_f.y,VehARR.P_r.x,VehARR.P_r.y);
            fprintf('Exp angle %f Curr angle %f\n',obj.Sign * exp_steer_wheel_angle , VehARR.steer_wheel_angle);
            fprintf('Cal angle deviate %f rate %f\n',angle_deviation,AngleRate);
        end
    end
end